#include <Arduino.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/cdefs.h>
#include <sys/time.h>
#include <time.h>

#include "esp_heap_caps.h"
#include "math.h"
#include "sdkconfig.h"
#include "soc/rtc.h"
#include "soc/soc.h"

#ifdef PLATFORMIO
#include "esp32-hal.h"
#else
#include "hal/cpu_hal.h"
#include "hal/gpio_hal.h"
#endif

/*******************************
 *    warning!!!: any copy of this code or his part must include this:
 *  "The original was written by Dima Samsonov @ Israel sdima1357@gmail.com on 3/2021" *
 *  Copyright (C) 2021  Dmitry Samsonov *
 ********************************/

/*\
   Changes by tobozo (sept 2021)
     - Backported commits from https://github.com/sdima1357/esp32_usb_soft_host/commit/7c51138ec1ea37d4dd5363d3f25a8e7ab8716871
     - ESP32-C3 can't work with Arduino IDE ( CONFIG_ESP_SYSTEM_MEMPROT_FEATURE=y ), C++ wrapper still valid through platformio/esp-idf though
   Changes by tobozo (may 2021):
     - Backported calibration at init (last changes from from Samsonov's repo)
   Changes by tobozo (march 2021):
     - Arduino IDE compliance (mostly code regression to esp-idf 3.x)
     - Added callbacks (data and device detection)
   Changes by tobozo (aug 2022):
     - Quick & dirty cpuDelay() to enable ESP32-S2 support when CONFIG_ESP_SYSTEM_MEMPROT_FEATURE is enabled (default on Arduino IDE)
     - Some reformatting and refactoring

\*/

#include "soft_usb.h"

// Arduino IDE complains about volatile at init, but we don't care
#pragma GCC diagnostic ignored "-Wdiscarded-qualifiers"
#pragma GCC diagnostic ignored "-Wunused-variable"

#define T_START 0b00000001
#define T_ACK 0b01001011
#define T_NACK 0b01011010
#define T_SOF 0b10100101
#define T_SETUP 0b10110100
#define T_DATA0 0b11000011
#define T_DATA1 0b11010010
#define T_DATA2 0b11100001
#define T_OUT 0b10000111
#define T_IN 0b10010110

#define T_ERR 0b00111100
#define T_PRE 0b00111100
#define T_NYET 0b01101001
#define T_STALL 0b01111000

// local non std
#define T_NEED_ACK 0b01111011
#define T_CHK_ERR 0b01111111

#define USB_LS_K 0
#define USB_LS_J 1
#define USB_LS_S 2

// most counters- uint_8t :  so  prevents overflow...

#define DEF_BUFF_SIZE 0x100

// somethins short  like ACK
#define SMALL_NO_DATA 36

int TRANSMIT_TIME_DELAY = 110; // delay each bit transmit
int TIME_MULT = 25;            // received time factor delta clocks* TIME_MULT/TIME_SCALE
int TM_OUT = 64;               // receive time out no activity on bus
#define TIME_SCALE 1024

// #define TEST
#ifdef TEST
#define TOUT 1000
#else
#define TOUT (TM_OUT)
#endif

uint32_t _getCycleCount32() {
    uint32_t ccount = cpu_hal_get_cycle_count();
    return ccount;
}
uint8_t _getCycleCount8d8(void) {
    uint32_t ccount = cpu_hal_get_cycle_count();
    return ccount >> 3;
}

#define SE_K                        \
    {                               \
        *snd[0][0] = (1 << DM_PIN); \
        *snd[0][1] = (1 << DP_PIN); \
    }
#define SE_J                        \
    {                               \
        *snd[1][0] = (1 << DM_PIN); \
        *snd[1][1] = (1 << DP_PIN); \
    }
#define SE_0                        \
    {                               \
        *snd[2][0] = (1 << DM_PIN); \
        *snd[2][1] = (1 << DP_PIN); \
    }

#if CONFIG_IDF_TARGET_ESP32 || CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3
#define SET_I                                             \
    {                                                     \
        PIN_INPUT_ENABLE(GPIO_PIN_MUX_REG[DP_PIN]);       \
        PIN_INPUT_ENABLE(GPIO_PIN_MUX_REG[DM_PIN]);       \
        GPIO.enable_w1tc = (1 << DP_PIN) | (1 << DM_PIN); \
    }
#define SET_O                                             \
    {                                                     \
        GPIO.enable_w1ts = (1 << DP_PIN) | (1 << DM_PIN); \
        PIN_INPUT_DISABLE(GPIO_PIN_MUX_REG[DP_PIN]);      \
        PIN_INPUT_DISABLE(GPIO_PIN_MUX_REG[DM_PIN]);      \
    }
#define READ_BOTH_PINS (((GPIO.in & RD_MASK) << 8) >> RD_SHIFT)
uint32_t *snd[4][2] =
    {
        {(uint32_t *)&GPIO.out_w1tc, (uint32_t *)&GPIO.out_w1ts}, // K
        {(uint32_t *)&GPIO.out_w1ts, (uint32_t *)&GPIO.out_w1tc}, // J
        {(uint32_t *)&GPIO.out_w1tc, (uint32_t *)&GPIO.out_w1tc}, // SE0
        {(uint32_t *)&GPIO.out_w1tc, (uint32_t *)&GPIO.out_w1tc}};
#else // ESP32C3
#define SET_I                                       \
    {                                               \
        PIN_INPUT_ENABLE(GPIO_PIN_MUX_REG[DP_PIN]); \
        PIN_INPUT_ENABLE(GPIO_PIN_MUX_REG[DM_PIN]); \
        gpio_ll_output_disable(&GPIO, DM_PIN);      \
        gpio_ll_output_disable(&GPIO, DP_PIN);      \
    }
#define SET_O                                                 \
    {                                                         \
        GPIO.enable_w1ts.val = (1 << DP_PIN) | (1 << DM_PIN); \
        PIN_INPUT_DISABLE(GPIO_PIN_MUX_REG[DP_PIN]);          \
        PIN_INPUT_DISABLE(GPIO_PIN_MUX_REG[DM_PIN]);          \
    }
#define READ_BOTH_PINS (((GPIO.in.val & RD_MASK) << 8) >> RD_SHIFT)
uint32_t *snd[4][2] =
    {
        {&GPIO.out_w1tc.val, &GPIO.out_w1ts.val},
        {&GPIO.out_w1ts.val, &GPIO.out_w1tc.val},
        {&GPIO.out_w1tc.val, &GPIO.out_w1tc.val},
        {&GPIO.out_w1tc.val, &GPIO.out_w1tc.val}};
#endif

// must be setup each time with setPins
uint32_t DP_PIN;
uint32_t DM_PIN;

uint32_t DM_PIN_M;
uint32_t DP_PIN_M;
uint16_t M_ONE;
uint16_t P_ONE;
uint32_t RD_MASK;
uint32_t RD_SHIFT;
// end must be setup each time with setPins

// temporary used insize lowlevel
volatile uint8_t received_NRZI_buffer_bytesCnt;
uint16_t received_NRZI_buffer[DEF_BUFF_SIZE];

volatile uint8_t transmit_bits_buffer_store_cnt;
// uint8_t transmit_bits_buffer_store[DEF_BUFF_SIZE];
uint8_t *transmit_bits_buffer_store = (uint8_t *)&received_NRZI_buffer[0];

volatile uint8_t transmit_NRZI_buffer_cnt;
uint8_t transmit_NRZI_buffer[DEF_BUFF_SIZE];

volatile uint8_t decoded_receive_buffer_head;
volatile uint8_t decoded_receive_buffer_tail;
uint8_t decoded_receive_buffer[DEF_BUFF_SIZE];
// end temporary used insize lowlevel

void (*cpuDelay)() = NULL;

#if defined CONFIG_ESP_SYSTEM_MEMPROT_FEATURE || defined FORCE_TEMPLATED_NOPS || defined CONFIG_IDF_TARGET_ESP32S3
// memory protection enabled, MALLOC_CAP_EXEC can't be used
// use c++ templated nop() instead (see nops.hpp)
typedef void (*CpuDelay_t)();
extern CpuDelay_t nops[256];
void SoftUSB::setCPUDelay(uint8_t ticks) {
    cpuDelay = nops[ticks];
}

#else // memprot disabled, make use of MALLOC_CAP_EXEC

#if CONFIG_IDF_TARGET_ESP32 || CONFIG_IDF_TARGET_ESP32S2
#define MAX_DELAY_CODE_SIZE 0x280
#define SDELAYMALLOC(X) malloc(X)
#define SDELAYREALLOC(X, Y, Z) heap_caps_realloc(X, Y, Z)
#define SDELAYRMASK MALLOC_CAP_EXEC
#elif CONFIG_IDF_TARGET_ESP32C3
#define MAX_DELAY_CODE_SIZE 0x210
#define SDELAYMALLOC(X) malloc(X)
#define SDELAYREALLOC(X, Y, Z) heap_caps_realloc(X, Y, Z)
#define SDELAYRMASK MALLOC_CAP_32BIT | MALLOC_CAP_EXEC
#else
#error "Unsupported target"
#endif

#if defined DEBUG_ALL
#define DEBUG_OPCODE printf
#else
#define DEBUG_OPCODE (void)
#endif

void makeOpcodes(uint8_t *ptr, uint8_t ticks) {
#if CONFIG_IDF_TARGET_ESP32 || CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3
    DEBUG_OPCODE("Making xtensa Opcodes for ESP32/ESP32S2\n");
    // put head of delay procedure
    *ptr++ = 0x36;
    *ptr++ = 0x41;
    *ptr++ = 0;
    for (int k = 0; k < ticks; k++) {
        // put NOPs
        *ptr++ = 0x3d;
        *ptr++ = 0xf0;
    }
    // put tail of delay procedure
    *ptr++ = 0x1d;
    *ptr++ = 0xf0;
    *ptr++ = 0x00;
    *ptr++ = 0x00;
#elif CONFIG_IDF_TARGET_ESP32C3
    DEBUG_OPCODE("Making RISCV Opcodes for ESP32C3\n");
    // put head of delay procedure
    for (int k = 0; k < ticks; k++) {
        // put NOPs
        *ptr++ = 0x1;
        *ptr++ = 0x0;
    }
    // put tail of delay procedure
    *ptr++ = 0x82;
    *ptr++ = 0x80;
#else
#error "Unsupported target"
#endif
}

void setCPUDelay(uint8_t ticks) {
    uint8_t *pntS;

    DEBUG_OPCODE("Setting delay to %d ticks\n", ticks);

    size_t free_iram = heap_caps_get_free_size(MALLOC_CAP_EXEC);
    DEBUG_OPCODE("Free iram before alloc: %d\n", free_iram);

    // it can't execute but can read & write
    if (!cpuDelay) {
        DEBUG_OPCODE("Malloc MAX_DELAY_CODE_SIZE=%d\n", MAX_DELAY_CODE_SIZE);
        pntS = SDELAYMALLOC(MAX_DELAY_CODE_SIZE);
    } else {
        DEBUG_OPCODE("Realloc MAX_DELAY_CODE_SIZE=%d\n", MAX_DELAY_CODE_SIZE);
        pntS = SDELAYREALLOC(cpuDelay, MAX_DELAY_CODE_SIZE, MALLOC_CAP_8BIT);
    }

    if (!pntS) {
        printf("OOM!\nHalting...\n");
        while (1) {
            vTaskDelay(1);
        }
    }

    uint8_t *pnt = (uint8_t *)pntS;
    makeOpcodes(pnt, ticks);
    DEBUG_OPCODE("moving opecodes to executable memory segment\n");
    // it can't  write  but can read & execute

    free_iram = heap_caps_get_free_size(MALLOC_CAP_EXEC);
    DEBUG_OPCODE("Free iram after alloc and before realloc: %d\n", free_iram);

    cpuDelay = SDELAYREALLOC(pntS, MAX_DELAY_CODE_SIZE, SDELAYRMASK);

    free_iram = heap_caps_get_free_size(MALLOC_CAP_EXEC);
    DEBUG_OPCODE("Free iram after realloc to cap_exec: %d\n", free_iram);

    if (!cpuDelay) {
        printf("idf.py menuconfig\n Component config-> ESP System Setting -> Memory protectiom-> Disable.\n memory prot must be disabled!!!\n cpuDelay = %p,\nHalting...\n", cpuDelay);
        while (1) {
            vTaskDelay(1);
        }
    }
}

#endif

typedef struct
{
    uint8_t cmd;
    uint8_t addr;
    uint8_t eop;

    uint8_t dataCmd;
    uint8_t bmRequestType;
    uint8_t bmRequest;
    uint16_t wValue;
    uint16_t wIndex;
    uint16_t wLen;
} Req;

enum DeviceState {
    NOT_ATTACHED,
    ATTACHED,
    POWERED,
    _DEFAULT,
    ADDRESS,
    PARSE_CONFIG,
    PARSE_CONFIG1,
    PARSE_CONFIG2,
    PARSE_CONFIG3,
    POST_ATTACHED,
    RESET_COMPLETE,
    POWERED_COMPLETE,
    DEFAULT_COMPL
};

enum CallbackCmd {
    CB_CHECK,
    CB_RESET,
    CB_WAIT0,   //单纯等待
    CB_POWER,
    CB_Resume,
    CB_TICK,
    CB_IN,
    CB_IN_Ack,
    CB_OUT,
    CB_OUT2,
    CB_SETUP,
    CB_SETUP2,
    CB_SETUP_End,
    CB_WAIT1,   // 等待时发送帧信号
    CB_WAIT2    // 等待结束后检查信号线
};

enum EventID {
    EV_Detected,
    EV_Eject,
    EV_SetupIn,
    EV_GetReport,
    EV_SetReport,
    EV_ERR,
    EV_Resume
};

// Req rq;
typedef struct
{
    int isValid; // USB HOST是否有效（引脚是否有定义）
    int selfNum; // USB HOST编号
    int cnt;
    bool bInsert = false; // 是否确认插入
    uint8_t address;
    uint32_t DP;
    uint32_t DM;

    // 由cmd确定执行什么命令
    volatile int cb_Cmd;
    int cb_Cmd_tmp;
    // 由status确定执行到哪个步骤
    volatile int fsm_state;
    // 事务过程中的延时，单位ms
    int cmdTimeOut;
    volatile uint16_t wires_last_state;

    uint8_t response[DEF_BUFF_SIZE]; // 获取的数据
    int responseLen;                 // 获取的数据的长度
    Req rq;
    Req rqTmp;

    int counterNAck;
    int counterAck;

    volatile int bComplete;

    int numb_reps_errors_allowed;

    uint8_t acc_decoded_resp[DEF_BUFF_SIZE];
    uint8_t acc_decoded_resp_counter;

    int asckedReceiveBytes;
    int asckedReceiveBytesTmp;
    int transmitL1Bytes;
    uint8_t transmitL1[DEF_BUFF_SIZE];
    uint8_t transmitL1Tmp[DEF_BUFF_SIZE];

    uint8_t epMin = 0;              // 监听最小端点号
    uint8_t epMax = 0;              // 监听最大端点号
    uint8_t interval = 0;           // 扫描间隔(ms)
    volatile int8_t currentEp = -1; // 当前监听的端口号

    volatile int user_request_status = 0; // 0：没有要处理的事务； 1：待处理 2:正在处理事务

} sUsbContStruct;

sUsbContStruct *current;

static xQueueHandle usb_msg_queue = NULL;

class USBMessage {
public:
    int usbNum = 0;
    int eventId = 0;
    int valule = 0;
    int len = 0;
    uint8_t *pDdata = nullptr;
    void destory() {
        if (pDdata) {
            delete[] pDdata;
        }
    }
};

void restart() {
    transmit_NRZI_buffer_cnt = 0;
}

void decoded_receive_buffer_clear() {
    decoded_receive_buffer_tail = decoded_receive_buffer_head;
}

void decoded_receive_buffer_put(uint8_t val) {
    decoded_receive_buffer[decoded_receive_buffer_head] = val;
    decoded_receive_buffer_head++;
}

uint8_t decoded_receive_buffer_get() {
    return decoded_receive_buffer[decoded_receive_buffer_tail++];
}

uint8_t decoded_receive_buffer_size() {
    return (uint8_t)(decoded_receive_buffer_head - decoded_receive_buffer_tail);
}

uint8_t cal5() {
    uint8_t crcb;
    uint8_t rem;

    crcb = 0b00101;
    rem = 0b11111;

    for (int k = 16; k < transmit_bits_buffer_store_cnt; k++) {
        int rb = (rem >> 4) & 1;
        rem = (rem << 1) & 0b11111;

        if (rb ^ (transmit_bits_buffer_store[k] & 1)) {
            rem ^= crcb;
        }
    }
    return (~rem) & 0b11111;
}

uint32_t cal16() {
    uint32_t crcb;
    uint32_t rem;

    crcb = 0b1000000000000101;
    rem = 0b1111111111111111;

    for (int k = 16; k < transmit_bits_buffer_store_cnt; k++) {
        int rb = (rem >> 15) & 1;
        rem = (rem << 1) & 0b1111111111111111;

        if (rb ^ (transmit_bits_buffer_store[k] & 1)) {
            rem ^= crcb;
        }
    }
    return (~rem) & 0b1111111111111111;
}

void seB(int bit) {
    transmit_bits_buffer_store[transmit_bits_buffer_store_cnt++] = bit;
}

void pu_MSB(uint16_t msg, int N) {
    for (int k = 0; k < N; k++) {
        seB(msg & (1 << (N - 1 - k)) ? 1 : 0);
    }
}

void pu_LSB(uint16_t msg, int N) {
    for (int k = 0; k < N; k++) {
        seB(msg & (1 << (k)) ? 1 : 0);
    }
}

void repack() {
    int last = USB_LS_J;
    int cntOnes = 0;
    transmit_NRZI_buffer[transmit_NRZI_buffer_cnt++] = USB_LS_J;
    for (int k = 0; k < transmit_bits_buffer_store_cnt; k++) {
        if (transmit_bits_buffer_store[k] == 0) {
            if (last == USB_LS_J || last == USB_LS_S) {
                last = USB_LS_K;
            } else {
                last = USB_LS_J;
            }
            cntOnes = 0;
        } else if (transmit_bits_buffer_store[k] == 1) {
            cntOnes++;
            if (cntOnes == 6) {
                transmit_NRZI_buffer[transmit_NRZI_buffer_cnt] = last;
                transmit_NRZI_buffer_cnt++;
                if (last == USB_LS_J) {
                    last = USB_LS_K;
                } else {
                    last = USB_LS_J;
                }
                cntOnes = 0;
            }
            if (last == USB_LS_S) {
                last = USB_LS_J;
            }
        }
        transmit_NRZI_buffer[transmit_NRZI_buffer_cnt++] = last;
    }

    transmit_NRZI_buffer[transmit_NRZI_buffer_cnt++] = USB_LS_S;
    transmit_NRZI_buffer[transmit_NRZI_buffer_cnt++] = USB_LS_S;

    transmit_NRZI_buffer[transmit_NRZI_buffer_cnt++] = USB_LS_J;
    transmit_NRZI_buffer[transmit_NRZI_buffer_cnt++] = USB_LS_J;
    transmit_NRZI_buffer[transmit_NRZI_buffer_cnt++] = USB_LS_J;
    transmit_NRZI_buffer[transmit_NRZI_buffer_cnt++] = USB_LS_J;

    transmit_bits_buffer_store_cnt = 0;
}

uint8_t rev8(uint8_t j) {
    uint8_t res = 0;
    for (int i = 0; i < 8; i++) {
        res <<= 1;
        res |= (j >> i) & 1;
    }
    return res;
}

uint16_t rev16(uint16_t j) {
    uint16_t res = 0;
    for (int i = 0; i < 16; i++) {
        res <<= 1;
        res |= (j >> i) & 1;
    }
    return res;
}

#ifdef DEBUG_ALL
uint16_t debug_buff[0x100];
#endif

int parse_received_NRZI_buffer() {

    if (!received_NRZI_buffer_bytesCnt)
        return 0;

    uint32_t crcb;
    uint32_t rem;

    crcb = 0b1000000000000101;
    rem = 0b1111111111111111;

    int res = 0;
    int cntOnes = 0;

    int terr = 0;
    uint8_t current_res = 0xfe;
    uint16_t prev = received_NRZI_buffer[0];
    int start = -1;
    uint8_t prev_smb = M_ONE;
#ifdef DEBUG_ALL
    debug_buff[0] = received_NRZI_buffer_bytesCnt;
    uint8_t rcnt = 1;
    debug_buff[received_NRZI_buffer_bytesCnt] = 0xff;
#endif
    for (int i = 1; i < received_NRZI_buffer_bytesCnt; i++) {
        // define 2.5
        uint16_t curr = (prev & 0xff00) + (((received_NRZI_buffer[i] - prev)) & 0xff);
        prev = received_NRZI_buffer[i];

        uint8_t smb = curr >> 8;
        int tm = (curr & 0xff);
        // debug_buff[i] = tm | (smb<<8);
        if (tm < 2 || (smb == 0)) {
            // terr+=tm<4?tm : 4;
            terr += tm;
        } else {
            // terr = 0;
            int delta = ((((curr + terr) & 0xff)) * TIME_MULT + TIME_SCALE / 2) / TIME_SCALE;

            for (int k = 0; k < delta; k++) {
                int incc = 1;

                if (prev_smb != smb) {
                    if (cntOnes != 6) {
                        current_res = current_res * 2 + 0;
                    } else {
                        incc = 0;
                    }
                    cntOnes = 0;
                } else {
                    current_res = current_res * 2 + 1;
                    cntOnes++;
                }
                if (start >= 0) {
                    start += incc;
                }
                if (current_res == 0x1 && start < 0) {
                    start = 0;
                }
                if ((start & 0x7) == 0 && incc) {
                    if (start == 8) {
                        res = current_res;
                    }
#ifdef DEBUG_ALL
                    debug_buff[rcnt++] = current_res;
#endif
                    decoded_receive_buffer_put(current_res);
                    if (start > 8) {
                        for (int bt = 0; bt < 8; bt++) {
                            int rb = (rem >> 15) & 1;
                            rem = (rem << 1) & 0b1111111111111111;
                            if (rb ^ ((current_res >> (7 - bt)) & 1)) {
                                rem ^= crcb;
                            }
                        }
                    }
                }

                prev_smb = smb;
            }
            terr = 0;
        }
    }
#ifdef DEBUG_ALL
    debug_buff[rcnt++] = 0xff;
#endif
    rem &= 0b1111111111111111;
    if (rem == 0b1111111111111111) {
        return res;
    }
    if (rem == 0x800d) {
        return T_NEED_ACK;
    } else {
        return T_CHK_ERR;
    }
}

void sendOnly() {

    static uint32_t sndA[4] = {0, 0, 0, 0};

    SET_O;

    uint32_t out_base = GPIO.out;
    sndA[0] = (out_base & ~DM_PIN_M) | DP_PIN_M;   // K
    sndA[1] = (out_base | DM_PIN_M) & ~DP_PIN_M;   // J
    sndA[2] = (out_base) & ~(DP_PIN_M | DM_PIN_M); // SE0
    sndA[3] = out_base | (DM_PIN_M | DP_PIN_M);    // N/A

    for (int i = 0; i < transmit_NRZI_buffer_cnt; i++) {
        // usb_transmit_delay(10);
        cpuDelay();
        GPIO.out = sndA[transmit_NRZI_buffer[i]];
    }
    restart();
    SET_I;
}

void sendRecieveNParse() {
    register uint32_t _R3;
    register uint16_t *STORE = received_NRZI_buffer;
    //__disable_irq();
    sendOnly();
    register uint32_t _R4; // = READ_BOTH_PINS;

START:
    _R4 = READ_BOTH_PINS;
    *STORE = _R4 | _getCycleCount8d8();
    STORE++;
    _R3 = _R4;
    //_R4 = READ_BOTH_PINS;
    // if(_R4!=_R3)  goto START;
    if (_R3) {
        for (int k = 0; k < TOUT; k++) {
            _R4 = READ_BOTH_PINS;
            if (_R4 != _R3)
                goto START;
        }
    }
    //__enable_irq();
    received_NRZI_buffer_bytesCnt = STORE - received_NRZI_buffer;
}

int sendRecieve() {
    sendRecieveNParse();
    return parse_received_NRZI_buffer();
}

void SOF() {
    if (1) {
        repack();
    }
    sendOnly();
}

void pu_Addr(uint8_t cmd, uint8_t addr, uint8_t eop) {
    pu_MSB(T_START, 8);
    pu_MSB(cmd, 8); // setup
    pu_LSB(addr, 7);
    pu_LSB(eop, 4);
    pu_MSB(cal5(), 5);
    repack();
}

void pu_ShortCmd(uint8_t cmd) {
    pu_MSB(T_START, 8);
    pu_MSB(cmd, 8); // setup
    pu_MSB(0, 16);
    repack();
}

void pu_Cmd(uint8_t cmd, uint8_t bmRequestType, uint8_t bmRequest, uint16_t wValue, uint16_t wIndex, uint16_t wLen) {
    pu_MSB(T_START, 8);
    pu_MSB(cmd, 8); // setup
    pu_LSB(bmRequestType, 8);
    pu_LSB(bmRequest, 8);
    pu_LSB(wValue, 16);
    pu_LSB(wIndex, 16);
    pu_LSB(wLen, 16);
    pu_MSB(cal16(), 16);
    repack();
}

uint8_t ACK_BUFF[0x20];
int ACK_BUFF_CNT = 0;

void ACK() {
    transmit_NRZI_buffer_cnt = 0;
    if (ACK_BUFF_CNT == 0) {
        pu_MSB(T_START, 8);
        pu_MSB(T_ACK, 8); // ack
        repack();
        memcpy(ACK_BUFF, transmit_NRZI_buffer, transmit_NRZI_buffer_cnt);
        ACK_BUFF_CNT = transmit_NRZI_buffer_cnt;
    } else {
        memcpy(transmit_NRZI_buffer, ACK_BUFF, ACK_BUFF_CNT);
        transmit_NRZI_buffer_cnt = ACK_BUFF_CNT;
    }
    sendOnly();
}

/*
前提：正确收到数据包时，必须在几微秒内返回ACK，否则USB设备会认为发送错误。
下次主机请求数据时，设备将回复同一数据包。

几微妙内要完成数据的校验是不可能的。因此：
收到设备发来的数据包，并不急于返回，而是进行校验和解码，如果通过的话，
再次向设备请求数据，等设备再次发来数据包时，不去校验，直接返回ACK。

由此，接收数据的过程分成一下几组：
grp1: IN 事务
        CB_IN: 发送IN令牌，接收数据。OK->CB_IN_Ack;
        CB_IN_Ack: 只要收到有内容的数据包就回复ACK，并结束过程

grp1: OUT 事务
        CB_OUT: 发送OUT令牌，接收ACK。OK->CB_OUT2;
        CB_OUT2: 发送数据，接收ACK。OK->结束过程;NG->CB_OUT2

grp2: SETUP 事务 (请求描述符,设置地址等)
        有的设备（可能仅限枚举阶段）发送数据后,不管主机是否回复ACK，
        下次IN请求时一定返回下一组数据。
        对策：收到后马上回复ACK，如果校验出错，则reset

        CB_SETUP：发送SETUP+DATA0令牌。 OK->CB_SETUP2;
        CB_SETUP2: 发送IN令牌，接收数据，检查是否收齐。NG->终止；收齐->CB_SETUP3; 未收齐:CB_SETUP2
        CB_SETUP3: 发送 DATA1(0B) 结束过程
*/
void SoftUSB::TransactionPiece() {
    decoded_receive_buffer_clear();

    if (current->cb_Cmd == CB_CHECK) { // 检查端口有无设备插入
        SET_I;
        current->wires_last_state = READ_BOTH_PINS >> 8;
        current->bComplete = 1;
    } else if (current->cb_Cmd == CB_RESET) { // 复位，使设备正常工作
        // SOF();
        // sendRecieveNParse();
        SET_O;
        SE_0;
        current->cmdTimeOut = 200;
        current->cb_Cmd = CB_WAIT0;
    } else if (current->cb_Cmd == CB_Resume) { // 唤醒USB设备
        SET_O;
        SE_K;
        SET_I;
        current->cmdTimeOut = 25;
        current->cb_Cmd = CB_WAIT0;
    } else if (current->cb_Cmd == CB_WAIT0) {
        if (current->cmdTimeOut > 0) {
            current->cmdTimeOut--;
        } else {
            // sendRecieveNParse();
            current->bComplete = 1;
        }
    } else if (current->cb_Cmd == CB_WAIT1) {
        SOF();
        if (current->cmdTimeOut > 0) {
            current->cmdTimeOut--;
        } else {
            sendRecieveNParse();
            current->wires_last_state = READ_BOTH_PINS >> 8;
            current->bComplete = 1;
        }
    } else if (current->cb_Cmd == CB_WAIT2) {
        if (current->cmdTimeOut > 0) {
            current->cmdTimeOut--;
        } else {
            current->wires_last_state = READ_BOTH_PINS >> 8;
            current->bComplete = 1;
        }
    } else if (current->cb_Cmd == CB_POWER) {
// for TEST
#ifdef TEST
        SOF();
        sendRecieve();
        SOF();
        SOF();
#else
        SET_O;
        SE_J;
        SET_I;
        current->cmdTimeOut = 30;
        current->cb_Cmd = CB_WAIT1;
#endif
    } else if (current->cb_Cmd == CB_TICK) {
        SOF();
        current->bComplete = 1;
    } else if (current->cb_Cmd == CB_OUT) {
        SOF();
        pu_Addr(current->rq.cmd, current->rq.addr, current->rq.eop);
        pu_Cmd(current->rq.dataCmd, current->rq.bmRequestType, current->rq.bmRequest, current->rq.wValue, current->rq.wIndex, current->rq.wLen);
        int res = sendRecieve();
        if (res == T_ACK) {
            current->cb_Cmd = CB_OUT2;
            current->numb_reps_errors_allowed = 1;
            return;
        } else {
            current->numb_reps_errors_allowed--;
            if (current->numb_reps_errors_allowed >= 0) {
                return;
            } else {
                current->cb_Cmd = CB_TICK;
                current->bComplete = 1;
            }
        }
    } else if (current->cb_Cmd == CB_OUT2) {
        SOF();
        pu_Addr(T_OUT, current->rq.addr, current->rq.eop);
        // reB();
        pu_MSB(T_START, 8);
        pu_MSB(T_DATA1, 8); // setup
        for (int k = 0; k < current->transmitL1Bytes; k++) {
            pu_LSB(current->transmitL1[k], 8);
        }
        pu_MSB(cal16(), 16);
        repack();
        sendRecieveNParse();
        pu_Addr(T_IN, current->rq.addr, current->rq.eop);
        // setup
        sendRecieveNParse();
        if (received_NRZI_buffer_bytesCnt < SMALL_NO_DATA && received_NRZI_buffer_bytesCnt > SMALL_NO_DATA / 4) {
            ACK();
        } else {
            current->numb_reps_errors_allowed--;
            if (current->numb_reps_errors_allowed >= 0) {
                return;
            } else {
            }
        }
        current->cb_Cmd = CB_TICK;
        current->bComplete = 1;
    } else if (current->cb_Cmd == CB_SETUP) {
        SOF();
        // 发送 setup包
        pu_Addr(current->rq.cmd, current->rq.addr, current->rq.eop);
        pu_Cmd(current->rq.dataCmd, current->rq.bmRequestType, current->rq.bmRequest, current->rq.wValue, current->rq.wIndex, current->rq.wLen);
        sendRecieveNParse();
        if (received_NRZI_buffer_bytesCnt < SMALL_NO_DATA           //
            && received_NRZI_buffer_bytesCnt > SMALL_NO_DATA / 4) { //

            current->cb_Cmd = CB_SETUP2;
            current->numb_reps_errors_allowed = 0;
            current->counterAck++;
            return;
        } else {
            // SOF();
            current->counterNAck++;
            current->numb_reps_errors_allowed--;
            if (current->numb_reps_errors_allowed >= 0) {
                // current->cb_Cmd = CB_TICK;
                current->acc_decoded_resp_counter = 0;
                return;
            } else {
                current->cb_Cmd = CB_TICK;
                current->bComplete = 1;
            }
        }
    } else if (current->cb_Cmd == CB_SETUP2) {
        SOF();
        // SETUP后的IN
        pu_Addr(T_IN, current->rq.addr, current->rq.eop);
        sendRecieveNParse();

        if (received_NRZI_buffer_bytesCnt >= SMALL_NO_DATA / 2) {
            // 只要收到数据，不管校验结果，先回复ACK
            ACK();
        } else if (current->asckedReceiveBytes == 0                        //
                   && current->acc_decoded_resp_counter == 0               //
                   && received_NRZI_buffer_bytesCnt < SMALL_NO_DATA        //
                   && received_NRZI_buffer_bytesCnt > SMALL_NO_DATA / 4) { //
            // 如果不是获取描述符的话(例如设置地址)，
            // 只需要返回0B数据就可以结束SETUP事务
            ACK();
            current->cb_Cmd = CB_TICK;
            current->bComplete = 1;
            return;
        }

        int res = parse_received_NRZI_buffer();
        if (res == T_NEED_ACK) {

            if (decoded_receive_buffer_size() > 2) {
                decoded_receive_buffer_get(); // 跳过同步域
                decoded_receive_buffer_get(); // 跳过PID域

                int bytes = decoded_receive_buffer_size() - 2;
                // Serial.printf("%d\n", bytes);
                for (int kk = 0; kk < bytes; kk++) {
                    current->acc_decoded_resp[current->acc_decoded_resp_counter] = rev8(decoded_receive_buffer_get());
                    current->acc_decoded_resp_counter++;
                    current->asckedReceiveBytes--;
                }
                if (bytes <= 0) {
                    // 有预期，但没收到
                    current->acc_decoded_resp_counter = 0;
                    current->asckedReceiveBytes = 0;
                    current->cb_Cmd = CB_TICK;
                    current->bComplete = 1;
                } else {
                    // 收到有效数据
                    if (current->asckedReceiveBytes <= 0) {
                        // 收齐
                        current->cb_Cmd = CB_SETUP_End;
                    }
                    return;
                }
            } else {
                // 有预期，但没收到
                current->acc_decoded_resp_counter = 0;
                current->asckedReceiveBytes = 0;
                current->cb_Cmd = CB_TICK;
                current->bComplete = 1;
                return;
            }
        } else {
            // 校验出错
            current->cb_Cmd = CB_TICK;
            current->bComplete = 1;
        }
    } else if (current->cb_Cmd == CB_SETUP_End) {
        SOF();
        pu_Addr(T_OUT, current->rq.addr, current->rq.eop);
        pu_ShortCmd(T_DATA1);
        sendOnly();
        current->cb_Cmd = CB_TICK;
        current->bComplete = 1;
    } else if (current->cb_Cmd == CB_IN_Ack) {
        SOF();
        pu_Addr(T_IN, current->rq.addr, current->rq.eop);
        // setup
        sendRecieveNParse();
        if (received_NRZI_buffer_bytesCnt < SMALL_NO_DATA / 2) {
            // no data , seems NAK or something like this
            current->cb_Cmd = CB_TICK;
            current->bComplete = 1;
            return;
        }
        ACK();
        current->cb_Cmd = CB_TICK;
        current->bComplete = 1;
    } else if (current->cb_Cmd == CB_IN) {
        SOF();
        pu_Addr(T_IN, current->rq.addr, current->rq.eop);
        // setup
        sendRecieveNParse();
        if (received_NRZI_buffer_bytesCnt < SMALL_NO_DATA / 2) {
            // no data , seems NAK or something like this
            current->cb_Cmd = CB_TICK;
            current->bComplete = 1;
            return;
        }
        int res = parse_received_NRZI_buffer();
        if (res == T_NEED_ACK) {
            if (decoded_receive_buffer_size() > 2) {
                decoded_receive_buffer_get();
                decoded_receive_buffer_get();
                int bytes = decoded_receive_buffer_size() - 2;
                for (int kk = 0; kk < bytes; kk++) {
                    current->acc_decoded_resp[current->acc_decoded_resp_counter] = rev8(decoded_receive_buffer_get());
                    current->acc_decoded_resp_counter++;
                    current->asckedReceiveBytes--;
                }
            }
            current->asckedReceiveBytes = 0;
            current->cb_Cmd = CB_IN_Ack;
            return;
        } else {
            current->numb_reps_errors_allowed--;
            if (current->numb_reps_errors_allowed > 0) {
                return;
            } else {
                current->cb_Cmd = CB_TICK;
                current->bComplete = 1;
            }
        }
        current->cb_Cmd = CB_TICK;
        current->bComplete = 1;
        current->asckedReceiveBytes = 0;
    }
}

void RequestSetupIn(uint8_t addr, uint8_t eop, uint8_t dataCmd, uint8_t bmRequestType, uint8_t bmRequest, uint16_t wValue, uint16_t wIndex, uint16_t wLen, uint16_t waitForBytes) {
    current->rq.cmd = T_SETUP;
    current->rq.addr = addr;
    current->rq.eop = eop;
    current->rq.dataCmd = dataCmd;
    current->rq.bmRequestType = bmRequestType;
    current->rq.bmRequest = bmRequest;
    current->rq.wValue = wValue;
    current->rq.wIndex = wIndex;
    current->rq.wLen = wLen;

    current->numb_reps_errors_allowed = 16;
    current->asckedReceiveBytes = waitForBytes;
    current->acc_decoded_resp_counter = 0;
    current->cb_Cmd = CB_SETUP;
}

void RequestSetupOut(uint8_t addr, uint8_t eop, uint8_t dataCmd, uint8_t bmRequestType, uint8_t bmRequest, uint16_t wValue, uint16_t wIndex, uint16_t wLen, uint16_t transmitL1Bytes, uint8_t *data) {
    current->rq.cmd = T_SETUP;
    current->rq.addr = addr;
    current->rq.eop = eop;
    current->rq.dataCmd = dataCmd;
    current->rq.bmRequestType = bmRequestType;
    current->rq.bmRequest = bmRequest;
    current->rq.wValue = wValue;
    current->rq.wIndex = wIndex;
    current->rq.wLen = wLen;

    current->transmitL1Bytes = transmitL1Bytes;
    for (int k = 0; k < current->transmitL1Bytes; k++) {
        current->transmitL1[k] = data[k];
    }

    current->numb_reps_errors_allowed = 16;
    current->acc_decoded_resp_counter = 0;
    current->cb_Cmd = CB_OUT;
}

void RequestIn(uint8_t cmd, uint8_t addr, uint8_t eop, uint16_t waitForBytes) {
    current->rq.cmd = cmd;
    current->rq.addr = addr;
    current->rq.eop = eop;
    current->numb_reps_errors_allowed = 8;
    current->asckedReceiveBytes = waitForBytes;
    current->acc_decoded_resp_counter = 0;
    current->cb_Cmd = CB_IN;
}

void (*SoftUSB::onDetectCalback)(uint8_t usbNum, sDevDesc *devDesc, size_t data_len) = nullptr;
void SoftUSB::setOnDetect(OnDetect_t cb) {
    onDetectCalback = cb;
}

void (*SoftUSB::onEjectCalback)(uint8_t usbNum) = nullptr;
void SoftUSB::setOnEject(OnEject_t cb) {
    onEjectCalback = cb;
}

void (*SoftUSB::onRecvDataCalback)(uint8_t usbNum, uint8_t *data, size_t data_len) = nullptr;
void SoftUSB::setOnRecvData(OnRecvData_t cb) {
    onRecvDataCalback = cb;
}

void (*SoftUSB::onErrorCalback)(uint8_t usbNum, int errorCode) = nullptr;
void SoftUSB::setOnError(OnError_t cb) {
    onErrorCalback = cb;
}

void (*SoftUSB::onBusResumeCalback)() = nullptr;
void SoftUSB::setOnBusResume(OnBusResume_t cb) {
    onBusResumeCalback = cb;
}

/*
1.检查并连接
2.发送/查询报表 (fsm_state=99~104)
*/
void SoftUSB::TransactionControl() {
    static unsigned long tLasScan = 0;
    if (!current->bComplete)
        return;
    current->bComplete = 0;

    if (current->fsm_state == 0) {
        current->cb_Cmd = CB_CHECK;
        current->currentEp == -1;
        current->address = 0;
        current->user_request_status = 0;
        current->fsm_state = 1;
    } else if (current->fsm_state == 1) {
        if (current->wires_last_state == M_ONE || current->wires_last_state == P_ONE) {
            // 检测到有设备插入
            current->cmdTimeOut = 100;
            current->cb_Cmd = CB_WAIT0;
            current->fsm_state = 2;
        } else {
            if (current->bInsert) {
                onEvent(current->selfNum, EV_Eject, 0, nullptr, 0);
                current->bInsert = false;
            }
            current->fsm_state = 0;
            current->cb_Cmd = CB_CHECK;
        }
    } else if (current->fsm_state == 2) {
        current->cb_Cmd = CB_CHECK;
        current->fsm_state = 3;
    } else if (current->fsm_state == 3) {
        // 确认稳定插入
        if (current->wires_last_state == M_ONE) { // low speed
            // 重置
            current->cb_Cmd = CB_RESET;
            current->fsm_state = 5;
        }  else if (current->wires_last_state == P_ONE) { // full speed
            // 不支持，等待拔出
            current->cmdTimeOut = 5;
            current->cb_Cmd = CB_WAIT2;
            current->fsm_state = 190;
            onEvent(current->selfNum, EV_Detected, 1, nullptr, 0);
            current->bInsert = true;
        } else {
            // 未插入或不支持的全速设备
            current->fsm_state = 0;
            current->cb_Cmd = CB_CHECK;
        }
    } else if (current->fsm_state == 5) {
        current->cb_Cmd = CB_POWER;
        current->fsm_state = 6;

    } else if (current->fsm_state == 6) {
        // 初次请求设备描述符
        RequestSetupIn(ZERO_USB_ADDRESS, 0b0000, T_DATA0, 0x80, 0x6, 0x0100, 0x0000, 0x0008, 0x0008);
        current->fsm_state = 7;
    } else if (current->fsm_state == 7) {
        if (current->acc_decoded_resp_counter) {
            onEvent(current->selfNum, EV_Detected, 9, current->acc_decoded_resp, current->acc_decoded_resp_counter);
        } else {
            //  失败，退回初始状态——认为没有连接
            if (current->numb_reps_errors_allowed <= 0) {
                current->fsm_state = 0;
                return;
            }
        }
        // 如果获取设备描述符失败，则跳过
        current->bInsert = true;
        current->fsm_state = 11;
    } else if (current->fsm_state == 11) {
        // 设置设备的地址
        RequestSetupIn(ZERO_USB_ADDRESS, 0b0000, T_DATA0, 0x00, 0x5, 0x0000 + ASSIGNED_USB_ADDRESS, 0x0000, 0x0000, 0x0000);
        current->fsm_state = 12;

    } else if (current->fsm_state == 12) {
        current->address = ASSIGNED_USB_ADDRESS;
        current->cmdTimeOut = 5;
        current->cb_Cmd = CB_WAIT1;
        current->fsm_state = 13;

        current->bInsert = true;
    } else if (current->fsm_state == 13) {
        // 请求设备描述符
        RequestSetupIn(current->address, 0b0000, T_DATA0, 0x80, 0x6, 0x0100, 0x0000, 0x0012, 0x0012);
        current->fsm_state = 14;
    } else if (current->fsm_state == 14) {
        if (current->acc_decoded_resp_counter == 0x12) {
            onEvent(current->selfNum, EV_Detected, 0, current->acc_decoded_resp, current->acc_decoded_resp_counter);
        } else {
            //  失败，退回初始状态——认为没有连接
            if (current->numb_reps_errors_allowed <= 0) {
                // onEvent(current->selfNum, 99, 1, nullptr, 0);
                current->fsm_state = 0;
                return;
            }
        }
        // 如果获取设备描述符失败，则跳过
        current->fsm_state = 100;
    } else if (current->fsm_state == 100) {
        if (current->user_request_status) {
            // 如果有事务的指令则执行
            // 事务中收到的数据将暂存到response[]
            if (current->rqTmp.cmd == T_SETUP) {
                switch (current->cb_Cmd_tmp) {
                case CB_SETUP: // 获取描述符，或设置配置、接口、地址等
                    current->user_request_status = 2;
                    current->fsm_state = 210;
                    break;
                case CB_OUT: // 设置报表(LED)
                    current->user_request_status = 2;
                    current->fsm_state = 220;
                    break;
                }
            }
        } else {
            // 如果没有指令则尝试读取
            current->fsm_state = 104;
        }

    } else if (current->fsm_state == 104) {
        if (current->user_request_status == 2) {
            current->user_request_status = 0;
        }
        //  查询报表
        if (current->currentEp == -1) {
            current->cmdTimeOut = 2;
            current->cb_Cmd = CB_WAIT1;
            current->fsm_state = 108;
        } else if (millis() - tLasScan >= current->interval) {
            tLasScan = millis();
            RequestIn(T_IN, current->address, current->currentEp, 8);
            current->fsm_state = 105;
        }
    } else if (current->fsm_state == 105) {
        // 收到报表
        if (current->acc_decoded_resp_counter >= 1) {
            onEvent(current->selfNum, EV_GetReport, current->currentEp, current->acc_decoded_resp, current->acc_decoded_resp_counter);
        }
        current->currentEp++;
        if (current->currentEp <= current->epMax) {
            //  查询报表
            RequestIn(T_IN, current->address, current->currentEp, 8);
        } else {
            current->currentEp = current->epMin;
            current->cmdTimeOut = 2;
            current->cb_Cmd = CB_WAIT1;
            current->fsm_state = 108;
        }
    } else if (current->fsm_state == 108) {
        current->cmdTimeOut = 4;
        current->cb_Cmd = CB_WAIT1;
        if (current->wires_last_state != M_ONE) {
            current->fsm_state = 0;
            return;
        }
        current->fsm_state = 100;
    } else if (current->fsm_state == 190) {
        current->cmdTimeOut = 100;
        current->cb_Cmd = CB_WAIT2;
        if (current->wires_last_state != P_ONE) {
            current->fsm_state = 0;
            return;
        }
    } else if (current->fsm_state == 210) {
        // SETUP + IN (获取描述符)
        memcpy(&current->rq, &current->rqTmp, sizeof(Req));
        current->asckedReceiveBytes = current->asckedReceiveBytesTmp;
        current->numb_reps_errors_allowed = 16;
        current->acc_decoded_resp_counter = 0;
        current->cb_Cmd = current->cb_Cmd_tmp;

        current->fsm_state = 211;
    } else if (current->fsm_state == 211) {
        if (current->acc_decoded_resp_counter == current->asckedReceiveBytesTmp) {
            //  获取到完整的IN数据(描述符)
            memcpy(current->response, current->acc_decoded_resp, current->acc_decoded_resp_counter);
            current->responseLen = current->acc_decoded_resp_counter;

            current->fsm_state = 104;
        } else {
            // 收不到报表描述符-->算了——以后在对应
            current->cmdTimeOut = 5;
            current->cb_Cmd = CB_WAIT1;
            current->fsm_state = 104;
        }
    } else if (current->fsm_state == 220) {
        // 设置键盘灯
        memcpy(&current->rq, &current->rqTmp, sizeof(Req));
        memcpy(&current->transmitL1, &current->transmitL1Tmp, current->transmitL1Bytes);
        current->numb_reps_errors_allowed = 8;
        current->acc_decoded_resp_counter = 0;
        current->asckedReceiveBytes = 0;
        current->cb_Cmd = current->cb_Cmd_tmp;

        current->fsm_state = 104;
    } else if (current->fsm_state == 230) {
        // bus resume
        current->cb_Cmd = CB_Resume;
        current->fsm_state = 104;
    } else {
        current->cmdTimeOut = 2;
        current->cb_Cmd = CB_WAIT1;
        current->fsm_state = 0;
    }
}

void setPins(int DPPin, int DMPin) {
    DP_PIN = DPPin;
    DM_PIN = DMPin;
    int diff = DPPin - DMPin;
    if (abs(diff) > 7) {
        printf("PIN DIFFERENCE MUST BE LESS THAN 8!\n");
        return;
    }
    int MIN_PIN = (DPPin < DMPin) ? DPPin : DMPin;

    DM_PIN_M = (1 << DMPin);
    DP_PIN_M = (1 << DPPin);
    RD_MASK = (1 << DPPin) | (1 << DMPin);

    RD_SHIFT = MIN_PIN;
    M_ONE = 1 << (DM_PIN - MIN_PIN);
    P_ONE = 1 << (DP_PIN - MIN_PIN);
}

sUsbContStruct current_usb[NUM_USB];

int checkPins(int dp, int dm) {
    int diff = abs(dp - dm);
    if (dp < 0 || dm < 0)
        return 0;

    if (diff > 7 || diff == 0) {
        printf("Diff not in [1-7] range: %d (%d-%d)\n", diff, dp, dm);
        return 0;
    }

    if (dp < 8 || dp > 31) {
        printf("WARN: Dp not in [8-31] range: [%d]\n", dp);
        return 0;
    }
    if (dm < 8 || dm > 31) {
        printf("WARN: Dm not in [8-31] range: [%d]\n", dp);
        return 0;
    }

    return 1;
}

int64_t get_system_time_us() {
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (tv.tv_sec * 1000000LL + (tv.tv_usec));
}

float testDelay6(float freq_MHz) {
// 6 bits must take 4.0 uSec
#define SEND_BITS 120
#define REPS 40
    float res = 1;
    transmit_NRZI_buffer_cnt = 0;

    for (int k = 0; k < SEND_BITS / 2; k++) {
        transmit_NRZI_buffer[transmit_NRZI_buffer_cnt++] = USB_LS_K;
        transmit_NRZI_buffer[transmit_NRZI_buffer_cnt++] = USB_LS_J;
    }

    int64_t stimb = get_system_time_us();
    for (int k = 0; k < REPS; k++) {
        sendOnly();
        transmit_NRZI_buffer_cnt = SEND_BITS;
    }

    uint32_t stim = get_system_time_us() - stimb;
    freq_MHz = 1.0f;
    res = stim * 6.0 / freq_MHz / (SEND_BITS * REPS);
    printf("%d bits in %f uSec %f MHz  6 ticks in %f uS\n", (SEND_BITS * REPS), stim / (float)freq_MHz, (SEND_BITS * REPS) * freq_MHz / stim, stim * 6.0 / freq_MHz / (SEND_BITS * REPS));

    return res;
}

uint8_t arr[0x200];

void setupGPIO(int pin) {
    // gpio_pad_select_gpio(pin);
    // gpio_set_direction(pin, GPIO_MODE_OUTPUT);
    // gpio_set_level(pin, 0);
    // gpio_set_direction(pin, GPIO_MODE_INPUT);
    // gpio_pulldown_en(pin);
    pinMode(pin, OUTPUT);
    digitalWrite(pin, LOW);
    pinMode(pin, INPUT_PULLDOWN);
}

bool SoftUSB::init(int DP0, int DM0, int DP1, int DM1, int DP2, int DM2, int DP3, int DM3) {
    if (inited)
        return false;

#if !defined USE_NATIVE_GROUP_TIMERS
    Serial.println("Using custom group timer");
    timer_queue = xQueueCreate(10, sizeof(timer_event_t));
#endif

    Serial.println("Creating message queue");

    usb_msg_queue = xQueueCreate(20, sizeof(struct USBMessage));

    Serial.println("Setting USB Delay");

    setCPUDelay(4);

    Serial.println("Setting up pins");

    initStates(DP0, DM0, DP1, DM1, DP2, DM2, DP3, DM3);

    // Serial.println("Creating timer config");

#if defined ESP32 || defined ESP32S2
    timer_config_t config;
    config.divider = TIMER_DIVIDER;
    config.counter_dir = TIMER_COUNT_UP;
    config.counter_en = TIMER_PAUSE;
    config.alarm_en = TIMER_ALARM_EN;
    config.intr_type = TIMER_INTR_MAX;
    config.auto_reload = (timer_autoreload_t)1; // fix for ¬invalid conversion from 'int' to 'timer_autoreload_t'¬ thanks rudi ;-)
#elif defined ESP32C3
    timer_config_t config =
        {
            .clk_src = TIMER_SRC_CLK_XTAL, // TIMER_SRC_CLK_DEFAULT ?
            .alarm_en = TIMER_ALARM_EN,    // enable timer alarm
            .counter_en = TIMER_PAUSE,     // starts counting counter once timer_init called
            .intr_type = TIMER_INTR_MAX,
            .counter_dir = TIMER_COUNT_UP,      // counts from 0 to counter value
            .auto_reload = TIMER_AUTORELOAD_EN, // reloads counter automatically
            .divider = TIMER_DIVIDER};
// #elif defined ESP32S2
#else
#error "Invalid board"
#endif

    // Serial.println("Init timer");
    timer_init(TIMER_GROUP_0, TIMER_0, &config);
    timer_set_counter_value(TIMER_GROUP_0, TIMER_0, 0x00000000ULL);
    int cnt = (double)TIMER_INTERVAL0_SEC * TIMER_SCALE;
    Serial.printf("timer0: %d\n", cnt);
    timer_set_alarm_value(TIMER_GROUP_0, TIMER_0, 40000); // 每1ms调用
    Serial.println("Enable interrupt");
    timer_enable_intr(TIMER_GROUP_0, TIMER_0);
    Serial.println("Register ISR");
    // 每1ms调用
    timer_isr_register(TIMER_GROUP_0, TIMER_0, SoftUSB::timer0_isr, (void *)TIMER_0, intr_alloc_flags, NULL);
    Serial.println("Start timer");
    timer_start(TIMER_GROUP_0, TIMER_0);

    inited = true;

    // // 开启后台任务
    // xTaskCreatePinnedToCore(
    //     backend,             // 任务函数
    //     "soft_usb::backend", // 任务名称
    //     8 * 1024,            // 堆栈大小（字节）
    //     this,                // 参数
    //     1,                   // 优先级（与默认 loop() 相同）
    //     NULL,                // 任务句柄（可选）
    //     1                    // 核心编号（1）
    // );

    return true;
}

// 每1ms调用
void IRAM_ATTR SoftUSB::timer0_isr(void *para) {
    timer_group_clr_intr_status_in_isr(TIMER_GROUP_0, TIMER_0);

#if CONFIG_IDF_TARGET_ESP32C3 || defined ESP32C3
    cpu_ll_enable_cycle_count();
#endif
    for (int i = 0; i < NUM_USB; i++) {
        current = &current_usb[i];
        if (current->isValid) {
            setPins(current->DP, current->DM);
            TransactionPiece();
            TransactionControl();
        }
    }

    timer_group_enable_alarm_in_isr(TIMER_GROUP_0, TIMER_0);
}

void SoftUSB::initStates(int DP0, int DM0, int DP1, int DM1, int DP2, int DM2, int DP3, int DM3) {
    decoded_receive_buffer_head = 0;
    decoded_receive_buffer_tail = 0;
    transmit_bits_buffer_store_cnt = 0;

    int calibrated = 0;

    for (int k = 0; k < NUM_USB; k++) {

        current = &current_usb[k];

        if (k == 0) {
            current->DP = DP0;
            current->DM = DM0;
        } else if (k == 1) {
            current->DP = DP1;
            current->DM = DM1;
        } else if (k == 2) {
            current->DP = DP2;
            current->DM = DM2;
        } else if (k == 3) {
            current->DP = DP3;
            current->DM = DM3;
        }

        current->isValid = 0;

        if (checkPins(current->DP, current->DM)) {

            printf("USB#%d (pins %d %d) is OK!\n", k, current->DP, current->DM);

            current->selfNum = k;
            current->address = 0;
            current->user_request_status = 0;
            current->bComplete = 1;
            current->cmdTimeOut = 0;
            current->cb_Cmd = 0;
            current->fsm_state = 0;
            current->wires_last_state = 0;
            current->counterNAck = 0;
            current->counterAck = 0;
            current->bInsert = false;
            current->epMin = 0;
            current->epMax = 0;
            current->currentEp = -1;

            setupGPIO(current->DP);
            setupGPIO(current->DM);

            current->isValid = 1;

            // TEST
            setPins(current->DP, current->DM);
            printf("READ_BOTH_PINS = %04x\n", READ_BOTH_PINS);
            SET_O;
            SE_0;
            SE_J;
            SE_0;
            SET_I;
            printf("READ_BOTH_PINS = %04x\n", READ_BOTH_PINS);
            pinMode(current->DP, OUTPUT);
            pinMode(current->DM, OUTPUT);
            printf("READ_BOTH_PINS = %04x\n", READ_BOTH_PINS);
            SET_I;
            printf("READ_BOTH_PINS = %04x\n", READ_BOTH_PINS);

            if (!calibrated) {
// calibrate delay divide 2
#define DELAY_CORR 2
                int uTime = 254;
                int dTime = 0;

                rtc_cpu_freq_config_t out_config;

                rtc_clk_cpu_freq_get_config(&out_config);
                printf("cpu freq = %d MHz\n", out_config.freq_mhz);

                TM_OUT = out_config.freq_mhz / 2;

                // 8  - func divided clock to 8, 1.5 - MHz USB LS
                TIME_MULT = (int)(TIME_SCALE / (out_config.freq_mhz / 8 / 1.5) + 0.5);
                printf("TIME_MULT = %d \n", TIME_MULT);

                int TRANSMIT_TIME_DELAY_OPT = 0;
                TRANSMIT_TIME_DELAY = TRANSMIT_TIME_DELAY_OPT;
                printf("D=%4d ", TRANSMIT_TIME_DELAY);
                setCPUDelay(TRANSMIT_TIME_DELAY);
                float cS_opt = testDelay6(out_config.freq_mhz);

#define OPT_TIME (4.00f)
                for (int p = 0; p < 9; p++) {
                    TRANSMIT_TIME_DELAY = (uTime + dTime) / 2;
                    printf("D=%4d ", TRANSMIT_TIME_DELAY);
                    setCPUDelay(TRANSMIT_TIME_DELAY);
                    float cS = testDelay6(out_config.freq_mhz);
                    if (fabsf(OPT_TIME - cS) < fabsf(OPT_TIME - cS_opt)) {
                        cS_opt = cS;
                        TRANSMIT_TIME_DELAY_OPT = TRANSMIT_TIME_DELAY;
                    }
                    if (cS < OPT_TIME) {
                        dTime = TRANSMIT_TIME_DELAY;
                    } else {
                        uTime = TRANSMIT_TIME_DELAY;
                    }
                }
                TRANSMIT_TIME_DELAY = TRANSMIT_TIME_DELAY_OPT + DELAY_CORR;
                setCPUDelay(TRANSMIT_TIME_DELAY);
                printf("TRANSMIT_TIME_DELAY = %d time = %f error = %f%% \n", TRANSMIT_TIME_DELAY, cS_opt, (cS_opt - OPT_TIME) / OPT_TIME * 100);
            }
        } else {
            if (current->DP == -1 && current->DM == -1) {
                printf("USB#%d is disabled by user configuration\n", k);
            } else {
                printf("USB#%d (pins %d %d) has errors and will be disabled !\n", k, current->DP, current->DM);
            }
        }
    }
}

#pragma GCC diagnostic pop

void SoftUSB::TimerPause() {
    if (!paused) {
        // log_d("Pausing timer");
        timer_pause(TIMER_GROUP_0, TIMER_0);
        paused = true;
        vTaskDelay(1);
    } else {
        // log_e("Timer already paused!");
    }
}

void SoftUSB::TimerResume() {
    if (paused) {
        // log_d("Resuming timer");
        timer_start(TIMER_GROUP_0, TIMER_0);
        paused = false;
        vTaskDelay(1);
    } else {
        // log_e("Timer already running!");
    }
}

// 总线挂起
void SoftUSB::BusSuspend() {
    TimerPause();
}
// 总线恢复
void SoftUSB::BusResume() {
    TimerResume();
    // for (int i = 0; i < NUM_USB; i++) {
    //     sUsbContStruct *pCurrent = &current_usb[i];
    //     if (pCurrent->isValid) {
    //         user_request(pCurrent->selfNum, UC_Resume, 0, 0);
    //     }
    // }
}

// 设置监听的端点号范围
void SoftUSB::setEndPoint(int usbNum, uint8_t epMin, uint8_t epMax, uint8_t interval) {

    Serial.printf("SoftUSB::setEndPoint: #%d: min:%d max:%d\n", usbNum, epMin, epMax);

    sUsbContStruct *pCurrent = &current_usb[usbNum];
    pCurrent->epMin = epMin;
    pCurrent->epMax = epMax;
    pCurrent->interval = interval;
    if (epMin > 0) {
        pCurrent->currentEp = epMin;
    }
}

/*
通知
*/
void SoftUSB::checkEvent() {
    static USBMessage msg;

    if (xQueueReceive(usb_msg_queue, &msg, 0)) {
        sUsbContStruct *pcurrent = &current_usb[msg.usbNum];
        if (msg.eventId == EV_Detected) { // 插入设备
            if (msg.valule == 1){
                // Full Speed
                Serial.printf("Unsupported Full Speed device: USB#%d\n", msg.usbNum);
            } else if (msg.valule == 9){
                // 首次取得设备描述符成功
                Serial.printf("Got dev desc: USB#%d\n", msg.usbNum);
            } else {
                // low speed
                pcurrent->epMin = 0;
                pcurrent->epMin = 0;
                pcurrent->currentEp = -1;

                if (onDetectCalback) {
                    onDetectCalback(msg.usbNum, (sDevDesc *)msg.pDdata, msg.len);
                }
            }
        } else if (msg.eventId == EV_Eject) {
            if (onEjectCalback) {
                onEjectCalback(msg.usbNum);
            }
        } else if (msg.eventId == EV_ERR) { // 错误信息
            if (onErrorCalback) {
                onErrorCalback(msg.usbNum, msg.valule);
            }
        } else if (msg.eventId == EV_GetReport) { // 收到报表
            if (onRecvDataCalback) {
                onRecvDataCalback(msg.usbNum, msg.pDdata, msg.len);
            }
        } else if (msg.eventId == EV_Resume) { // on resume
            if (onBusResumeCalback) {
                onBusResumeCalback();
            }
        }
        msg.destory();
    }
}

// 为了尽快响应，把数据储存到队列后马上返回
// 队列里的数据，由backend线程读取
void SoftUSB::onEvent(int usbNum, int eventId, int value, uint8_t *dataBuf, int nLen) {
    USBMessage msg;
    msg.usbNum = usbNum;
    msg.eventId = eventId;
    msg.valule = value;
    msg.len = nLen;

    if (nLen > 0) {
        msg.pDdata = new uint8_t[nLen];
        memcpy(msg.pDdata, dataBuf, nLen);
    }
    xQueueSend(usb_msg_queue, (void *)&msg, (TickType_t)0);
}

uint8_t *SoftUSB::usbSetupIn(int usbNum, uint8_t *pData0, int waitForBytes, int *pResultLen) {
    sUsbContStruct *pcurrent = &current_usb[usbNum];

    // 等待其他事务处理完
    while (pcurrent->user_request_status) {
        vTaskDelay(1);
    }

    pcurrent->rqTmp.cmd = T_SETUP;
    pcurrent->rqTmp.addr = pcurrent->address;
    pcurrent->rqTmp.eop = 0;
    pcurrent->rqTmp.dataCmd = T_DATA0;
    pcurrent->rqTmp.bmRequestType = pData0[0];
    pcurrent->rqTmp.bmRequest = pData0[1];
    pcurrent->rqTmp.wValue = *(uint16_t *)&pData0[2];
    pcurrent->rqTmp.wIndex = *(uint16_t *)&pData0[4];
    pcurrent->rqTmp.wLen = *(uint16_t *)&pData0[6];

    pcurrent->asckedReceiveBytesTmp = waitForBytes;
    pcurrent->cb_Cmd_tmp = CB_SETUP;

    // 等待事务处理完
    pcurrent->user_request_status = 1;
    while (pcurrent->user_request_status) {
        vTaskDelay(1);
    }

    if (pResultLen) {
        *pResultLen = pcurrent->responseLen;
        if (*pResultLen) {
            uint8_t *p = new uint8_t[*pResultLen];
            memcpy(p, pcurrent->response, pcurrent->responseLen);
            return p;
        }
    }

    return nullptr;
}

void SoftUSB::usbSetupOut(int usbNum, uint8_t *pData0, uint8_t *pData1, int data1Len) {
    sUsbContStruct *pcurrent = &current_usb[usbNum];
    // 等待其他事务处理完
    while (pcurrent->user_request_status) {
        vTaskDelay(1);
    }

    pcurrent->rq.cmd = T_SETUP;
    pcurrent->rqTmp.addr = pcurrent->address;
    pcurrent->rqTmp.eop = 0;
    pcurrent->rqTmp.dataCmd = T_DATA0;
    pcurrent->rqTmp.bmRequestType = pData0[0];
    pcurrent->rqTmp.bmRequest = pData0[1];
    pcurrent->rqTmp.wValue = *(uint16_t *)&pData0[2];
    pcurrent->rqTmp.wIndex = *(uint16_t *)&pData0[4];
    pcurrent->rqTmp.wLen = *(uint16_t *)&pData0[6];

    pcurrent->transmitL1Bytes = data1Len;

    memcpy(pcurrent->transmitL1Tmp, pData1, data1Len);

    pcurrent->cb_Cmd_tmp = CB_OUT;

    // 等待事务处理完
    pcurrent->user_request_status = 1;
    while (pcurrent->user_request_status) {
        vTaskDelay(1);
    }
}

SoftUSB _SUSB;